#include "Arduino.h"
#include "Ping.h"

UltrasonicSensor::UltrasonicSensor(int echo, int trigger, unsigned int max_Time){
    _cm_factor = 58;
	_echo_Pin = echo;
    _trigger_Ping = trigger;
	_max_Time = max_Time * _cm_factor; 
	
	pinMode(_trigger_Ping, OUTPUT);
    pinMode(_echo_Pin, INPUT);
}

/*
Gets the distance from an object and converts it to cm.
There are other functions and librarys that do thus.
They take up close to 10% of the total memory.
*/
unsigned int UltrasonicSensor::getDistance(){
    digitalWrite(_trigger_Ping, LOW);
    delayMicroseconds(4);
    digitalWrite(_trigger_Ping, HIGH);
    delayMicroseconds(10);
    digitalWrite(_trigger_Ping, LOW);
    unsigned int duration = pulseIn(_echo_Pin, HIGH, _max_Time);
  #ifdef DEBUG
      Serial.print("Ping before conversuion: ");
      Serial.println(duration);
  #endif
  // convert the time into a distance
  return (duration / _cm_factor); 
}